import numpy as np
import math
import dtw



def split_ts_seq(pos, mag):
    """

    :param ts_seq:
    :param sep_ts:
    :return:
    """
    tss_pos = pos[:, 0].astype(float)
    tss_mag = mag[:, 0].astype(float)
    
    # print(tss_mag)
    unique_sep_ts = np.unique(tss_pos)
    # print(unique_sep_ts)
    ts_seqs = []
    for i in range(0, unique_sep_ts.shape[0]):
        index = np.searchsorted(tss_mag, unique_sep_ts[i], side='left') #从时间的左边开始找两个时间最近的在磁数据的下索引
        ts_seqs.append(index)
    # print(ts_seqs)
    return np.hstack((pos,mag[:,1:][np.array(ts_seqs)])) #拼接矩阵 a,b [a,b]


def blh2xyz84(b, la, h):
    """blh 转 xyz
    
    Args:
        b: 纬度
        la: 经度
        h: 高
        
    Returns:
        x y z Cartesian coordinates (meters)
    """
    b   = b*math.pi / 180
    # %  la   = lambda(1) + lambda(2)/60 + lambda(3)/3600
    la   = la*math.pi / 180
    a = 6378137
    f = 1/298.257223563
    ex2 = (2-f)*f
    N = a / math.sqrt(1 - ex2*math.sin(b)**2)
    X = (N+h) * math.cos(b) * math.cos(la)
    Y = (N+h) * math.cos(b) * math.sin(la)
    Z = ((1-ex2)*N + h) * math.sin(b)

    return np.array([X, Y, Z])

def  Latitude2Meridian(B, RefEllipsoid):
# %子午线长计算
# %X 单位 m
# %B 单位 rad
    import math
    a = RefEllipsoid[0]
    b = RefEllipsoid[1]
    e_2 = RefEllipsoid[4]
    e = e_2**0.5

    m0 = a*(1-e_2)
    m2 = 3*e_2*m0/2
    m4 = 5*e_2*m2/4
    m6 = 7*e_2*m4/6.0
    m8 = 9*e_2*m6/8.0

    a0 = m0 + m2/2.0 + m4*3/8.0 + m6*5/16.0 + m8*35/128.0
    a2 = m2/2.0 + m4/2 + m6*15/32.0 + m8*7/16.0
    a4 = m4/8 + m6*3/16.0 + m8*7/32.0
    a6 = m6/32.0 + m8/16.0
    a8 = m8/128.0

    X = a0*B - a2*math.sin(B*2)*0.5 + a4*math.sin(B*4)*0.25 - a6*math.sin(B*6)/6.0 + a8*math.sin(B*8)*0.125
    return X
def BL2Gauss(B, L):
    import math
    a=6378137.0000
    b=6356752.3142
    c = a*a/b
    alpha = (a-b)/a
    e_2 = (a*a-b*b)/(a*a)
    e_2_ = (a*a-b*b)/(b*b) #e'_2
    RefEllipsoid = [a, b, c, alpha, e_2, e_2_]


    L0=116
    L=L-L0

    B=B*math.pi/180.0
    L=L*math.pi/180.0

    #%B = B / p                       %将B由秒化为弧度
    sinB = math.sin(B)
    cosB = math.cos(B)
    cosB_2 = cosB*cosB
    cosB_3 = cosB_2*cosB
    cosB_5 = cosB_2*cosB_3

    L_2 = L*L
    L_3 = L_2*L
    L_4 = L_2*L_2
    L_5 = L_4*L
    L_6 = L_4*L_2

    t = math.tan(B)
    t_2 = t*t
    t_4 = t_2*t_2

    n_2 = e_2_*cosB_2
    n_4 = n_2*n_2

    N = a * ( 1-e_2*sinB*sinB )**-0.5

    X = Latitude2Meridian(B, RefEllipsoid)

    x = X + N*sinB*cosB*L_2/2 + N*sinB*cosB_3*(5 - t_2 + 9*n_2 +4*n_4)*L_4/24 + N*sinB*cosB_5*(61 - 58*t_2 + t_4)*L_6/720
    #% x=X+N*t*cosB_2*L_2/2 + N*t*cosB*cosB_3*(5 - t_2 + 9*n_2 +4*n_4)*L_4/24 + N*t*cosB*cosB_5*(61 - 58*t_2 + t_4)*L_6/720
    y = N*cosB*L + N*cosB_3*(1 - t_2 + n_2)*L_3/6 + N*cosB_5*(5- 18*t_2 + t_4 + 14*n_2 - 58*n_2*t_2)*L_5/120
    return x,y
def dtw_distance(ts_a, ts_b, d=lambda x,y: abs(x-y), mww=10000):
    """Computes dtw distance between two time series. DP问题
    
    Args:
        ts_a: time series a
        ts_b: time series b
        d: distance function
        mww: max warping window, int, optional (default = infinity)
        
    Returns:
        dtw distance
    """
    
    # Create cost matrix via broadcasting with large int
    ts_a, ts_b = np.array(ts_a), np.array(ts_b)
    M, N = len(ts_a), len(ts_b)
    cost = np.ones((M, N))

    # Initialize the first row and column
    cost[0, 0] = d(ts_a[0], ts_b[0])
    for i in range(1, M):
        cost[i, 0] = cost[i-1, 0] + d(ts_a[i], ts_b[0])

    for j in range(1, N):
        cost[0, j] = cost[0, j-1] + d(ts_a[0], ts_b[j])

    # Populate rest of cost matrix within window
    for i in range(1, M):
        for j in range(max(1, i - mww), min(N, i + mww)):
            choices = cost[i-1, j-1], cost[i, j-1], cost[i-1, j]
            cost[i, j] = min(choices) + d(ts_a[i], ts_b[j])

    # Return DTW distance given window 
    return cost[-1, -1]


# 位姿转换，矩阵->向量，向量->矩阵
def v2t(v):
    '''
    file    :compute_f.py
    descri  :向量->矩阵
    time    :2021/09/09 10:39:50
    author :binyao
    version :1.0
    input   :(x,y,theta(弧度制))
    output  :
    '''
    # v[2] = math.radians(wrapTo180(v[2]))
    c = np.cos(v[2])
    s = np.sin(v[2])
    A = [[c, -s, v[0]],
        [s, c, v[1]],
        [0, 0, 1]]
    return np.array(A)

def t2v(A):
    '''
    file    :compute_f.py
    descri  :矩阵->向量
    time    :2021/09/09 10:40:03
    author :binyao
    version :1.0
    input   :
    output  :(x,y,theta)
    '''
    import math
    v = np.zeros(3)
    v[0:2] = A[0:2,2]
    v[2] = math.atan2(A[1,0], A[0,0])
    # v[2] = math.degrees(v[2])%360
    return v

def wrapTo180(lon):

    return lon if lon <= 180 else lon - 360

def magSequence3DTo1(s1,s1_mag):
    '''
    file    :compute_f.py
    descri  :N x 3的三轴磁向量阵转为3N x 1的磁向量
    time    :2021/11/11 14:36:12
    author :binyao
    version :1.0
    input   :  
            s1:# 0 X, 1 Y, 2 h, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
            s1_mag: 全0的向量
    output  :
    '''
    for i in range(len(s1)):
        j = i*3
        s1_mag[j] = s1[i,4]
        s1_mag[j+1] = s1[i,5]
        s1_mag[j+2] = s1[i,6]

def alignmentIndexVote(a, b):
    '''
    file    :compute_f.py
    descri  :
    time    :2021/11/11 19:33:00
    author :binyao
    version :1.0
    input   :
            a: 原 3N x 1的磁向量的索引
            b：N x 1的磁向量的索引
    output  :
    '''
    for i in range(len(b)):
        j = i*3
        b[i] = sum(a[j:j+3])//9
